#include "usr_inc.h"

/* NRF点位信息的缓存区 */
PoseMsg_t nrfPose = {0};
/* ywj增加的关于nrf按键的标志位 */
uint8_t Get_Flag;
/**
 * @brief 限制摇杆数据
 */
int16_t NRF_LimitJoy(int16_t val)
{
    /*遥控器发生了更改*/
    if (val < -1950) return -1950;
    if (val > 1950) return 1950;
    if (val >= -195 && val <= 195) return 0;
    return val;
}

/**
 * @brief 初始化NRF
 */
void ALL_NRF_Init(void)
{
    while (NRF24L01_Check())
    {
        ;
    }
    NRF24L01_Init_Rx();
}

void StartNrfUpdate(void *argument)
{
    for (uint16_t period = 1; ;)
    {
        uint8_t data[9];
        const uint8_t res = NRF_Recv(data);


        /* 推射指令 */
        if (res == 0xAA)
        {
            if(Get_Flag == 0)
            {
                if (data[1] == '>')
                {
                    TossCmdMsg_t cmd;
                    if (data[2] == 'L')
                    {
                        cmd = TOSS_DRAG;
                        xQueueSendToBack(tossCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'F')
                    {
                        cmd = TOSS_FREE;
                        xQueueSendToBack(tossCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'N')
                    {
                        cmd = TOSS_ABANDON;
                        xQueueSendToBack(tossCmdQueueHandle, &cmd, 1);
                    }
                }
                Get_Flag=1;
            }
        }
        /* 运球指令 */
        else if (res == 0xAC)
        {
            if(Get_Flag == 0)
            {
                if (data[1] == ']')
                {
                    DribbleCmdMsg_t cmd;
                    if (data[2] == 'S')
                    {
                        cmd = DRIBBLE_EXTEND;
                        xQueueSendToBack(dribbleCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'H')
                    {
                        cmd = DRIBBLE_RETRACT;
                        xQueueSendToBack(dribbleCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'X')
                    {
                        cmd = DRIBBLE_DRAW;
                        xQueueSendToBack(dribbleCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'L')
                    {
                        cmd = DRIBBLE_FREE;
                        xQueueSendToBack(dribbleCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'P')
                    {
                        cmd = DRIBBLE_PAT;
                        xQueueSendToBack(dribbleCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'W')
                    {
                        cmd = DRIBBLE_HOLD;
                        xQueueSendToBack(dribbleCmdQueueHandle, &cmd, 1);
                    }
                }
                Get_Flag=1;
            }
        }
        /* 推程数据 */
        else if (res == 0xDE)
        {
            if(Get_Flag == 0)
            {
                int32_t rawS;
                memcpy(&rawS, data + 1, sizeof(int32_t));

                float stroke = (float)rawS / 1000000.0;
                xQueueSendToBack(tossStrokeQueueHandle, &stroke, 1);
                Get_Flag=1;
            }
        }
        /* 摇杆数据 */
        else if (res == 0x02)
        {
            /* ywj加的 */
            Get_Flag = 0;
            /*遥控器发生了更改*/
            int16_t lx = (((uint16_t)data[2] & 0x0F) << 8) | data[1]; // 设置的是输出12bit, 因此需要把16bit的高4位去掉, 所以需要&0x0F
            int16_t ly = (((uint16_t)data[4] & 0x0F) << 8) | data[3];
            int16_t ry = (((uint16_t)data[6] & 0x0F) << 8) | data[5];
            int16_t rx = (((uint16_t)data[8] & 0x0F) << 8) | data[7];

            JoyStickMsg_t msg = {0};

            msg.leftX = NRF_LimitJoy(-lx + 2084);
            msg.leftY = NRF_LimitJoy(ly - 1974);
            msg.rightX = NRF_LimitJoy(-rx + 2037);
            msg.rightY = NRF_LimitJoy(ry - 2126);

            xQueueSendToBack(joyMsgQueueHandle, &msg, 1);
        }
        /* 增加的部分是有关按键的 */
        else if(res == 'K')
        {
            // printf("111\r\n");
            if(Get_Flag == 0){
                KeyCmdMsg_t msg;
                // printf("%d\r\n",data[1]);
                switch (data[1]) {
                case 1:
                    msg = KEY_1;
                    break;
                case 2:
                    msg = KEY_2;
                    break;
                case 3:
                    msg = KEY_3;
                    break;
                case 4:
                    msg = KEY_4;
                    break;
                case 5:
                    msg = KEY_5;
                    break;
                case 6:
                    msg = KEY_6;
                    break;
                case 7:
                    msg = KEY_7;
                    break;
                case 8:
                    msg = KEY_8;
                    break;
                case 9:
                    msg = KEY_9;
                    break;
                case 10:
                    msg = KEY_10;
                    break;
                default:
                    break;
                }
                Get_Flag=1;
                xQueueSendToBack(keyCmdQueueHandle, &msg, 1);
            }
        }
        else if(res == 1)
        {
            if(Get_Flag==0)
                {
                // printf("%d,%d\n",data[1],data[2]);
                }
            Get_Flag=1;
        }
        /* 底盘指令 */
        else if (res == 0xBB)
        {
            if(Get_Flag == 0)
            {
                if (data[1] == 'C')
                {
                    ChassisCmdMsg_t cmd;
                    if (data[2] == 'L')
                    {
                        cmd = CHASSIS_LOCK;
                        xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'F')
                    {
                        cmd = CHASSIS_FREE;
                        xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'Y')
                    {
                        cmd = CHASSIS_CONF_YAW;
                        xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'J')
                    {
                        cmd = CHASSIS_CONF_JOY;
                        xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'P')
                    {
                        cmd = CHASSIS_CONF_POINT;
                        xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                    }
                }
                if (data[1] == 'Y')
                {
                    ChassisCmdMsg_t cmd;
                    if (data[2] == 'F')
                    {
                        cmd = CHASSIS_Front;
                        xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'B')
                    {
                        cmd = CHASSIS_Back;
                        xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'L')
                    {
                        cmd = CHASSIS_Left;
                        xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'R')
                    {
                        cmd = CHASSIS_Right;
                        xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == '1')
                    {
                        cmd = CHASSIS_Right_Front;
                        xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == '2')
                    {
                        cmd = CHASSIS_Right_Back;
                        xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                    }
                }
                Get_Flag=1;
            }
        }
        /* 底盘点位信息 */
        else if (res == 0xBC)
        {
            if(Get_Flag == 0)
            {
                int32_t rawX, rawY;
                memcpy(&rawX, data + 1, sizeof(int32_t));
                memcpy(&rawY, data + 5, sizeof(int32_t));

                nrfPose.posX = (float)rawX / 1000.0;
                nrfPose.posY = (float)rawY / 1000.0;
                nrfPose.sum = rawX + rawY;
                Get_Flag=1;
            }
        }
        else if (res == 0xBD)
        {
            if(Get_Flag == 0)
            {
                int32_t rawYaw, rawSum;
                memcpy(&rawYaw, data + 1, sizeof(int32_t));
                memcpy(&rawSum, data + 5, sizeof(int32_t));

                nrfPose.yaw = (float)rawYaw / 1000.0;
                nrfPose.sum += rawYaw;
                if (nrfPose.sum == rawSum)
                {
                    xQueueSendToBack(chassisPoseCmdQueueHandle, &nrfPose,1);
                }
                memset(&nrfPose, 0, sizeof(PoseMsg_t));
                Get_Flag=1;
            }
        }
        else if (res == 0xCC)
        {
            if(Get_Flag == 0)
            {
                if (data[1] == 'P')
                {
                    YunqiuSai_Command_t cmd;
                    if (data[2] == 'S')
                    {
                        cmd = POINT_STOP;
                        xQueueSendToBack(YunqiuSaiCmdQueueHandle, &cmd, 1);
                    }
                    if (data[2] == '1')
                    {
                        cmd = POINT_1;
                        xQueueSendToBack(YunqiuSaiCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == '2')
                    {
                        cmd = POINT_2;
                        xQueueSendToBack(YunqiuSaiCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == '3')
                    {
                        cmd = POINT_3;
                        xQueueSendToBack(YunqiuSaiCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == '4')
                    {
                        cmd = POINT_4;
                        xQueueSendToBack(YunqiuSaiCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == '5')
                    {
                        cmd = POINT_5;
                        xQueueSendToBack(YunqiuSaiCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == '6')
                    {
                        cmd = POINT_6;
                        xQueueSendToBack(YunqiuSaiCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == '7')
                    {
                        cmd = POINT_7;
                        xQueueSendToBack(YunqiuSaiCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'Z')
                    {
                        cmd = POINT_DUIKANGSAI;
                        xQueueSendToBack(YunqiuSaiCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'Y')
                    {
                        cmd = POINT_STRAT_YUNQIUSAI;
                        xQueueSendToBack(YunqiuSaiCmdQueueHandle, &cmd, 1);
                    }
                    else if (data[2] == 'D')
                    {
                        cmd = POINT_DRIBBLE_TEST;
                        xQueueSendToBack(YunqiuSaiCmdQueueHandle, &cmd, 1);
                    }
                }
                Get_Flag=1;
            }
        }
        else if (res == 0xCD)
        {
            if(Get_Flag == 0)
            {
                if (data[1] == 'P')
                {
                    DribbleGame_Command_t cmd;
                    if (data[2] == 'A')
                    {
                        cmd = AUTO;
                        xQueueSendToBack(DribbleGameCmdQueueHandle, &cmd, 1);
                    }
                    if (data[2] == 'T')
                    {
                        cmd = TRY_AGAIN;
                        xQueueSendToBack(DribbleGameCmdQueueHandle, &cmd, 1);
                    }
                    if (data[2] == 'G')
                    {
                        cmd = GO_BACK;
                        xQueueSendToBack(DribbleGameCmdQueueHandle, &cmd, 1);
                    }
                    if (data[2] == 'P')
                    {
                        cmd = PREPARE;
                        xQueueSendToBack(DribbleGameCmdQueueHandle, &cmd, 1);
                    }
                    if (data[2] == 'C')
                    {
                        cmd = CIRCLE;
                        xQueueSendToBack(DribbleGameCmdQueueHandle, &cmd, 1);
                    }
                    if (data[2] == 'S')
                    {
                        cmd = STOP;
                        xQueueSendToBack(DribbleGameCmdQueueHandle, &cmd, 1);
                    }
                }
                Get_Flag=1;
            }
        }
        vTaskDelay(pdMS_TO_TICKS(period));
    }
}
